﻿Imports System.Timers
Imports System.Windows
Imports System.Net.Sockets
Imports System.Net

Public Class VirtualDS
    Dim _isOpen As Boolean
    Dim _transmitTimer As Timer
    Dim _lastReceivedPacket As RobotPacket
    Dim _lastSentPacket As DSPacket
    Dim _pendingPacket As DSPacket
    Dim _transmitClient As UdpClient
    Dim _receviceClient As UdpClient
    Dim _transmitRemoteEP As IPEndPoint
    Dim _transmitLocalEP As IPEndPoint
    Dim _receiveRemoteEP As IPEndPoint
    Dim _receiveLocalEP As IPEndPoint
    Dim _isConnected As Boolean = False
    Private _invalidPacketCount As Integer = 0
    Private _missedPacketCount As Integer = 0

    Public Sub New(ByVal teamNumber As Integer)
        _pendingPacket = New DSPacket(1)
        _pendingPacket.ControlData.RawData = 84

        _pendingPacket.TeamNumber = teamNumber
    End Sub

    Public Sub Open(ByVal teamNumber As Integer)
        _transmitRemoteEP = New IPEndPoint(GetIP(teamNumber, Devices.Robot), Configuration.DsToRobotRemotePortNumber)
        _transmitLocalEP = New IPEndPoint(GetIP(teamNumber, Devices.DS), Configuration.DsToRobotLocalPortNumber)

        _receiveRemoteEP = New IPEndPoint(GetIP(teamNumber, Devices.Robot), Configuration.RobotToDsRemotePortNumber)
        _receiveLocalEP = New IPEndPoint(GetIP(teamNumber, Devices.DS), Configuration.RobotToDsLocalPortNumber)

        _transmitClient = New UdpClient(_transmitLocalEP)
        _transmitTimer = New Timer(1000 / 60)

        _receviceClient = New UdpClient(_receiveLocalEP)
        _isOpen = True
        _receviceClient.BeginReceive(AddressOf Me.ReceiveData, Nothing)
        AddHandler _transmitTimer.Elapsed, AddressOf Me.SendData
        _transmitTimer.Enabled = True
    End Sub

    Public Sub SendData(ByVal sender As Object, ByVal e As ElapsedEventArgs)
        Try
            _transmitTimer.Enabled = False

            If _lastReceivedPacket IsNot Nothing Then
                MissedPacketCount = _lastSentPacket.PacketId - _lastReceivedPacket.ReplyId
                If _lastReceivedPacket.ReplyId > _lastSentPacket.PacketId Then
                    MissedPacketCount = Integer.MaxValue
                End If
            Else
                If _lastSentPacket IsNot Nothing Then
                    MissedPacketCount = _lastSentPacket.PacketId
                Else
                    MissedPacketCount = 0
                End If
            End If

            If MissedPacketCount > Configuration.MissedPacketCountSafety Or _
                InvalidPacketCount > Configuration.InvalidPacketCountSafety Then
                IsConnected = False
            Else
                IsConnected = True
            End If

            If Not IsConnected Then
                'Debug.WriteLine(_pendingPacket.ControlData.CRioCheckSum)
                Debug.WriteLine("Not connected")
                _pendingPacket.ControlData.Enabled = False
                _pendingPacket.ControlData.Resync = True
                _pendingPacket.ControlData.Reset = False
                _pendingPacket.ControlData.Autonomous = False
                _pendingPacket.ControlData.EStop = False
                _pendingPacket.ControlData.CRioCheckSum = False
                _pendingPacket.ControlData.FpgaCheckSum = False
            Else
                Debug.WriteLine("Connected")
                _pendingPacket.ControlData.Resync = False
                _pendingPacket.ControlData.Enabled = True
                _pendingPacket.ControlData.Autonomous = True
            End If

            Dim ctrlData = _pendingPacket.ControlData

            Dim sendData = _pendingPacket.GetBytes()
            _transmitClient.Send(sendData, sendData.Length, _transmitRemoteEP)
            _lastSentPacket = _pendingPacket
            _pendingPacket = _lastSentPacket.GetNextPacket()
            _transmitTimer.Enabled = True
        Catch ex As Exception
            Debug.WriteLine(ex.Message)
        Finally
            _transmitTimer.Enabled = True
        End Try
    End Sub

    Public Sub ReceiveData(ByVal ar As IAsyncResult)
        Try
            Dim endpoint As IPEndPoint = Nothing
            Dim bytes = _receviceClient.EndReceive(ar, endpoint)
            ParseBytes(bytes)
        Catch ex As Exception
            Throw
        End Try
    End Sub

    Private Sub ParseBytes(ByVal data As Byte())
        Try
            _lastReceivedPacket = New RobotPacket(data)

            If _lastReceivedPacket.IsValid Then
                InvalidPacketCount = 0
            Else
                InvalidPacketCount += 1
            End If
        Catch ex As Exception
            Debug.WriteLine(ex.Message)
            Debug.Assert(False)
            Throw
        Finally
            If _isOpen Then
                _receviceClient.BeginReceive(AddressOf Me.ReceiveData, Nothing)
            End If
        End Try
    End Sub

    Public Property IsConnected() As Boolean
        Get
            Return _isConnected
        End Get
        Set(ByVal value As Boolean)
            _isConnected = value
        End Set
    End Property
    Public ReadOnly Property PendingPacket() As DSPacket
        Get
            Return _pendingPacket
        End Get
    End Property

    Public ReadOnly Property LastPacket() As DSPacket
        Get
            Return _lastSentPacket
        End Get
    End Property


    Public Property MissedPacketCount() As Integer
        Get
            Return _missedPacketCount
        End Get
        Set(ByVal value As Integer)
            _missedPacketCount = value
        End Set
    End Property
    Public Property InvalidPacketCount() As Integer
        Get
            Return _invalidPacketCount
        End Get
        Set(ByVal value As Integer)
            _invalidPacketCount = value
        End Set
    End Property

End Class

